
#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "modbus/modbus.h"

#include "rclcpp/rclcpp.hpp"
#include "basic_sprayer_interfaces/msg/four_drivers.hpp" // 四个驱动
#include "basic_sprayer_interfaces/msg/four_wheel.hpp"   // 四轮速度
#include "basic_sprayer_interfaces/msg/pid.hpp"          // 四轮速度

#include <pthread.h>
#include <semaphore.h>

// 轮距 423mm
// 轴距 538mm

/*驱动器 的驱动代码  和 */
//  /dev/ttySC0 -->第一个串口
//  /dev/ttySC1 -->第二个串口

class driver
{
public:
    driver(modbus_t *ctx_bus, uint8_t modbus_id);
    ~driver();
    bool start(void); //启动
    bool stop(void);  //停止
    /*单位rpm*/
    bool write_speed(float speed);
    bool read_status(void);
    bool read_write(float speed);
    bool read_write_start_stop(int8_t start, float speed);

    uint32_t get_error_code(void);
    double get_temperature(void);
    double get_voltage(void);
    double get_current(void);
    double get_power(void);
    double get_speed(void);
    double get_position(void);

    bool set_pi(float p, float i);

    void set_start(int8_t _start)
    {
        start_set = _start;
    }
    int8_t get_start(void)
    {
        return start_set;
    }
    void set_goal_speed(float spd)
    {
        speed = spd;
    }
    float get_goal_speed(void)
    {
        return speed;
    }

private:
    bool table_update; //table数据是否更新了

    int8_t start_set;
    float speed; //

    int32_t error_code; //错误
    int16_t temperature_raw;
    int16_t voltage_raw;
    int16_t current_raw;
    int16_t power_raw;
    int32_t speed_raw;
    int32_t position_raw;

    modbus_t *ctx_bus; // 与实际的串口相连接
    uint8_t m_id;
    int8_t start_flag;           //启动标志位
    uint16_t device_id;          // 产品ID
    uint16_t firmware_version;   // 固件版本号
    uint16_t bootloader_version; // bootloader版本号
    uint16_t hardware_version;   // 硬件版本号
    /* 不同的软件 适配不同的硬件  这个需要 在开机启动的时候
        判断是否需要 */
    uint16_t serial_number[8]; // 序列号 大端模式 先放高位
    /* 信息表中的 参数转换系数 */
    float coefficient_pwm;     // 占空比转化系数
    float coefficient_voltage; // 有了系数就可以逆推出最大值了 但是我们一般不这样做
    float coefficient_current;
    float coefficient_power;
    float coefficient_temperature;
    float coefficient_rspeed;
    float coefficient_position;

    bool read_information_table(void);
    float int_2_float(uint16_t *data); //不知为何modbus 自带的函数会有问题
    void float_2_int(float value, uint16_t *data);
};

typedef union
{
    float float_v;
    uint16_t uint16_v[2];
} f_i_t;

float driver::int_2_float(uint16_t *data)
{
    f_i_t value;
    value.uint16_v[1] = data[0];
    value.uint16_v[0] = data[1];
    return value.float_v;
}

void driver::float_2_int(float value, uint16_t *data)
{
    f_i_t va;
    va.float_v = value;
    data[0] = va.uint16_v[1];
    data[1] = va.uint16_v[0];
}

/*构造函数*/
driver::driver(modbus_t *ctx_bus, uint8_t modbus_id)
{
    this->ctx_bus = ctx_bus;
    m_id = modbus_id;
    table_update = false;
    start_flag = false;
    read_information_table();
}

/**
* 更新信息表
* @author 何思伟
* @param 
*
* @note 
*
* @return 
*/
bool driver::read_information_table(void)
{
    modbus_set_slave(ctx_bus, m_id);
    uint16_t data[26] = {0};
    /*初始化信息表*/
    if (modbus_read_registers(ctx_bus, 0, 26, data) != -1)
    {
        device_id = data[0]; // 读到了 设备id
        firmware_version = data[1];
        bootloader_version = data[2];
        hardware_version = data[3];
        for (int i = 0; i < 8; i++)
        { // 序列号
            serial_number[i] = data[4 + i];
        }
        /* 转化系数 */
        // coefficient_pwm = modbus_get_float_dcba(&data[12]);
        // coefficient_voltage = modbus_get_float_dcba(&data[14]);
        // coefficient_current = modbus_get_float_dcba(&data[16]);
        // coefficient_power = modbus_get_float_dcba(&data[18]);
        // coefficient_temperature = modbus_get_float_dcba(&data[20]);
        // coefficient_rspeed = modbus_get_float_dcba(&data[22]);
        // coefficient_position = modbus_get_float_dcba(&data[24]);
        // printf("coefficient_rspeed = %f,d1=%d,d2=%d\n",coefficient_rspeed,data[22],data[23]);
        coefficient_pwm = int_2_float(&data[12]);
        coefficient_voltage = int_2_float(&data[14]);
        coefficient_current = int_2_float(&data[16]);
        coefficient_power = int_2_float(&data[18]);
        coefficient_temperature = int_2_float(&data[20]);
        coefficient_rspeed = int_2_float(&data[22]);
        coefficient_position = int_2_float(&data[24]);
        // printf("coefficient_rspeed = %f,d1=%d,d2=%d\n",coefficient_rspeed,data[22],data[23]);
        table_update = true;
        return true;
    }
    return false;
}

/**
* 启动电机
* @author 何思伟
* @param 
*
* @note 
*
* @return 
*/
bool driver::start()
{
    /**/
    modbus_set_slave(ctx_bus, m_id);
    uint16_t data[5] = {0};
    data[0] = 1;            //同步续流
    data[1] = (0 << 8) | 3; //速度模式
    data[2] = 1;            //启动
    if (modbus_write_registers(ctx_bus, 100, 5, data) != -1)
    {
        return true;
    }
    return false;
}
/**
* 停止电机
* @author 何思伟
* @param 
*
* @note 
*
* @return 
*/
bool driver::stop()
{
    modbus_set_slave(ctx_bus, m_id);
    /*写0 停下来*/
    if (modbus_write_register(ctx_bus, 102, 0) != -1)
    {
        return true;
    }
    return false;
}

/**
* 写目标速度
* @author 何思伟
* @param speed 单位是rpm
*
* @note 
*
* @return 
*/
bool driver::write_speed(float speed)
{
    modbus_set_slave(ctx_bus, m_id);
    if (table_update == false)
    {
        if (read_information_table() == false)
        {
            return false;
        }
    }
    int32_t max_speed = speed / coefficient_rspeed;
    uint16_t data[2] = {0};
    data[0] = max_speed >> 16;
    data[1] = max_speed & 0xFFFF;
    if (modbus_write_registers(ctx_bus, 103, 2, data) != -1)
    {
        return true;
    }
    return false;
}

/*读取状态*/
bool driver::read_status(void)
{
    modbus_set_slave(ctx_bus, m_id);
    uint16_t data[10] = {0};
    if (modbus_read_registers(ctx_bus, 130, 10, data) != -1)
    {
        error_code = (data[0] << 16) | data[1];
        temperature_raw = data[2];
        voltage_raw = data[3];
        current_raw = data[4];
        power_raw = data[5];
        speed_raw = (data[6] << 16) | data[7];
        position_raw = (data[8] << 16) | data[9];
        return true;
    }
    return false;
}

/*读写*/
bool driver::read_write(float speed)
{
    modbus_set_slave(ctx_bus, m_id);
    if (table_update == false)
    {
        if (read_information_table() == false)
        {
            return false;
        }
    }
    int32_t max_speed = speed / coefficient_rspeed;
    // printf("coefficient_rspeed = %f,length = %d\n",coefficient_rspeed,max_speed);
    uint16_t data[2] = {0};
    data[0] = max_speed >> 16;
    data[1] = max_speed & 0xFFFF;
    uint16_t value[10] = {0};
    if (modbus_write_and_read_registers(ctx_bus, 103, 2, data, 130, 10, value) != -1)
    {
        error_code = (value[0] << 16) | value[1];
        temperature_raw = value[2];
        voltage_raw = value[3];
        current_raw = value[4];
        power_raw = value[5];
        speed_raw = (value[6] << 16) | value[7];
        position_raw = (value[8] << 16) | value[9];
        return true;
    }
    return false;
}
/**
* 最终调用的启动或者是开始的api 
* @author 何思伟
* @param 
*
* @note 
*
* @return 
*/
bool driver::read_write_start_stop(int8_t start, float speed)
{
    modbus_set_slave(ctx_bus, m_id);
    if (table_update == false)
    {
        if (read_information_table() == false)
        {
            return false;
        }
    }
    if (start != start_flag)
    {
        if (start == 0)
        { //停止
            uint16_t data[2] = {0};
            uint16_t value[10] = {0};
            if (modbus_write_and_read_registers(ctx_bus, 102, 1, data, 130, 10, value) != -1)
            {
                error_code = (value[0] << 16) | value[1];
                temperature_raw = value[2];
                voltage_raw = value[3];
                current_raw = value[4];
                power_raw = value[5];
                speed_raw = (value[6] << 16) | value[7];
                position_raw = (value[8] << 16) | value[9];
                start_flag = start;
                return true;
            }
            return false;
        }
        else
        { //启动
            uint16_t data[5] = {0};
            data[0] = 1;            //同步续流
            data[1] = (0 << 8) | 3; //速度模式
            data[2] = 1;            //启动
            uint16_t value[10] = {0};
            if (modbus_write_and_read_registers(ctx_bus, 100, 5, data, 130, 10, value) != -1)
            {
                error_code = (value[0] << 16) | value[1];
                temperature_raw = value[2];
                voltage_raw = value[3];
                current_raw = value[4];
                power_raw = value[5];
                speed_raw = (value[6] << 16) | value[7];
                position_raw = (value[8] << 16) | value[9];
                start_flag = start;
                return true;
            }
        }
    }
    else //设置速度
    {
        if (start_flag == 0)
        {
            return read_status();
        }
        else
        {
            return read_write(speed);
        }
    }
    return false;
}
//0.03
//0.08
bool driver::set_pi(float p, float i)
{
    modbus_set_slave(ctx_bus, m_id);
    uint16_t data[4] = {0};
    if (modbus_read_registers(ctx_bus, 381, 4, data) != -1)
    {
        float gp = int_2_float(&data[0]);
        float gi = int_2_float(&data[2]);
        // RCLCPP_INFO(this->get_logger(), "gp=%f,gi=%f\n", gp, gi);
        if ((p != gp) || (i != gi))
        {
            float_2_int(p, &data[0]);
            float_2_int(i, &data[2]);
            modbus_write_registers(ctx_bus, 381, 4, data);
        }
    }
    return true;
}

uint32_t driver::get_error_code(void)
{
    return error_code;
}
double driver::get_temperature(void)
{
    return temperature_raw * coefficient_temperature;
}
double driver::get_voltage(void)
{
    return voltage_raw * coefficient_voltage;
}

double driver::get_current(void)
{
    return current_raw * coefficient_current;
}

double driver::get_power(void)
{
    return power_raw * coefficient_power;
}
double driver::get_speed(void)
{
    return speed_raw * coefficient_rspeed;
}
double driver::get_position(void)
{
    return position_raw * coefficient_position;
}

/*创建 ros node
    一个订阅者
    一个发布者
    发布驱动器的状态
    订阅转速信号
*/
using namespace std::chrono_literals;

#define NUM 4

pthread_cond_t cond_send = PTHREAD_COND_INITIALIZER; // 条件变量 触发通信
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;   //
sem_t wait_end;                                      // 等待结束

/* 与driver 通信的线程 */
void *driver_thread(void *arg)
{
    driver *dri = (driver *)arg;
    while (1)
    {
        /* 等待信号 */
        pthread_mutex_lock(&mutex);
        pthread_cond_wait(&cond_send, &mutex);
        pthread_mutex_unlock(&mutex);

        if (dri->read_write_start_stop(dri->get_start(), dri->get_goal_speed()) == false)
        {
            printf("ERROR:motor read failed!");
        }
        sem_post(&wait_end);
    }
    return 0;
}

class driverNode : public rclcpp::Node
{
public:
    driverNode() : Node("driver")
    {
        // modbus_t *ctx_global = modbus_new_rtu("/dev/ttySC0", 115200, 'N', 8, 1);
        // if (ctx_global != NULL)
        // {
        //     modbus_set_slave(ctx_global, 0x1);
        //     if (modbus_connect(ctx_global) == -1)
        //     {
        //         fprintf(stderr, "voltage device connection failed: %s\n", modbus_strerror(errno));
        //         modbus_free(ctx_global);
        //         ctx_global = NULL;
        //         return;
        //         /* 这里也是出错了 */
        //     }
        //     modbus_set_response_timeout(ctx_global, 0, 60000); //60ms
        //     modbus_set_error_recovery(ctx_global, MODBUS_ERROR_RECOVERY_PROTOCOL);
        // }
        // else
        // {
        //     printf("ERROR:can not create modbus in tty[%d]\n");
        //     return;
        // }

        // publisher_ = this->create_publisher<basic_sprayer_interfaces::msg::FourDrivers>("driver/status", 10);
        // subscription_ = this->create_subscription<basic_sprayer_interfaces::msg::FourWheel>(
        //     "driver/fourwheel", 10, std::bind(&driverNode::wheel_callback, this, std::placeholders::_1));
        // subscription_pid = this->create_subscription<basic_sprayer_interfaces::msg::Pid>(
        //     "pid", 10, std::bind(&driverNode::pid_callback, this, std::placeholders::_1));
        // timer_ = this->create_wall_timer(500ms, std::bind(&driverNode::timer_callback, this));

        // for (uint8_t i = 0; i < NUM; i++)
        // {
        //     driver_four[i] = new driver(ctx_global, i + 1);
        // }
        // comm_cnt = 0;
        /* 多线程同步运行 */
        // const char *tty[4] = {"/dev/usb_0", "/dev/usb_1", "/dev/usb_2", "/dev/ttySC0"}; //地址 0 1 2 3
        const char *tty[4] = {"/dev/usb_0", "/dev/usb_1", "/dev/usb_2", "/dev/usb_3"}; //地址 0 1 2 3
        // modbus_t *ctx_global = modbus_new_rtu(tty[3], 115200, 'N', 8, 1);
        // if (ctx_global != NULL)
        // {
        //     modbus_set_slave(ctx_global, 3 + 1);
        //     if (modbus_connect(ctx_global) == -1)
        //     {
        //         fprintf(stderr, "voltage device connection failed: %s\n", modbus_strerror(errno));
        //         modbus_free(ctx_global);
        //         ctx_global = NULL;
        //         return;
        //         /* 这里也是出错了 */
        //     }
        //     modbus_set_response_timeout(ctx_global, 0, 20000); //60ms
        //     // modbus_set_byte_timeout(ctx_global, 0, 95*2);   //
        //     modbus_set_error_recovery(ctx_global, MODBUS_ERROR_RECOVERY_PROTOCOL);
        // }
        // else
        // {
        //     printf("ERROR:can not create modbus in tty[%d]\n", 3);
        //     /* 进程结束了 */
        //     return;
        // }
        for (uint8_t i = 0; i < NUM; i++)
        {
            modbus_t *ctx_global = modbus_new_rtu(tty[i], 115200, 'N', 8, 1);
            if (ctx_global != NULL)
                {
                    modbus_set_slave(ctx_global, i + 1);
                    if (modbus_connect(ctx_global) == -1)
                    {
                        fprintf(stderr, "voltage device connection failed: %s\n", modbus_strerror(errno));
                        modbus_free(ctx_global);
                        ctx_global = NULL;
                        return;
                        /* 这里也是出错了 */
                    }
                    modbus_set_response_timeout(ctx_global, 0, 20000); //60ms
                    // modbus_set_byte_timeout(ctx_global, 0, 95*2);   //
                    modbus_set_error_recovery(ctx_global, MODBUS_ERROR_RECOVERY_PROTOCOL);
                }
            else
            {
                printf("ERROR:can not create modbus in tty[%d]\n", i);
                /* 进程结束了 */
                return;
            }
            driver_four[i] = new driver(ctx_global, i + 1);
            /* 创建三个线程 */
            if (i < 3)
            {
                pthread_attr_t attr;
                pthread_attr_init(&attr);
                pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
                int ret = pthread_create(&th_id1_3[i], &attr, driver_thread, driver_four[i]);
                if (ret != 0)
                {
                    printf("ERROR:create thread failed,error code:%d", ret);
                    return;
                }
            }
        }

        publisher_ = this->create_publisher<basic_sprayer_interfaces::msg::FourDrivers>("driver/status", 10);
        subscription_ = this->create_subscription<basic_sprayer_interfaces::msg::FourWheel>(
            "driver/fourwheel", 10, std::bind(&driverNode::wheel_callback, this, std::placeholders::_1));
        subscription_pid = this->create_subscription<basic_sprayer_interfaces::msg::Pid>(
            "pid", 10, std::bind(&driverNode::pid_callback, this, std::placeholders::_1));
        timer_ = this->create_wall_timer(500ms, std::bind(&driverNode::timer_callback, this));

        comm_cnt = 0;
    }
    /*测试指令回转的速度*/
    void test(void)
    {
        auto message = basic_sprayer_interfaces::msg::FourDrivers();
        while (1)
        {
            driver_four[3]->read_write(300);
            message.header.stamp = this->now();
            message.driver[3].error_code = driver_four[3]->get_error_code();
            message.driver[3].temperature_c = driver_four[3]->get_temperature();
            message.driver[3].bus_voltage_v = driver_four[3]->get_voltage();
            message.driver[3].current_a = driver_four[3]->get_current();
            message.driver[3].power_w = driver_four[3]->get_power();
            message.driver[3].speed_rpm = driver_four[3]->get_speed();
            message.driver[3].position_rad = driver_four[3]->get_position();
            publisher_->publish(message);
        }
    }

private:
    rclcpp::Publisher<basic_sprayer_interfaces::msg::FourDrivers>::SharedPtr publisher_;
    rclcpp::Subscription<basic_sprayer_interfaces::msg::FourWheel>::SharedPtr subscription_;
    rclcpp::TimerBase::SharedPtr timer_;

    rclcpp::Subscription<basic_sprayer_interfaces::msg::Pid>::SharedPtr subscription_pid;

    driver *driver_four[4]; // 四个驱动器

    int8_t comm_cnt; // 计数器

    pthread_t th_id1_3[3]; // 线程  地址1~3 使用这个线程

    /*回调函数*/
    void wheel_callback(const basic_sprayer_interfaces::msg::FourWheel::SharedPtr msg)
    {
        /*接收到信号*/
        float speed[4] = {0};
        int8_t start = msg->start_flag;
        speed[0] = msg->output_left_front;
        speed[1] = msg->output_right_front;
        speed[2] = msg->output_left_rear;
        speed[3] = msg->output_right_rear;
        /*准备返回的指令*/
        comm_cnt++;
        // RCLCPP_INFO(this->get_logger(),"start=%d",start);
        auto message = basic_sprayer_interfaces::msg::FourDrivers();

        driver_four[0]->set_start(start);
        driver_four[0]->set_goal_speed(speed[0]);
        driver_four[1]->set_start(start);
        driver_four[1]->set_goal_speed(speed[1]);
        driver_four[2]->set_start(start);
        driver_four[2]->set_goal_speed(speed[2]);

        /*唤醒所有的线程*/
        pthread_mutex_lock(&mutex);
        pthread_cond_broadcast(&cond_send);
        pthread_mutex_unlock(&mutex);
        if (driver_four[3]->read_write_start_stop(start, speed[3]) == false)
        {
            RCLCPP_INFO(this->get_logger(), "motor%d read_write_start_stop failed!", 3);
        }
        /* 等待所有线程结束 */
        sem_wait(&wait_end);
        sem_wait(&wait_end);
        sem_wait(&wait_end);
        for (uint8_t i = 0; i < NUM; i++)
        {
            // if (driver_four[i]->read_write_start_stop(start, speed[i]) == false)
            // {
            //     RCLCPP_INFO(this->get_logger(), "motor%d read_write_start_stop failed!", i);
            // }
            message.driver[i].header.stamp = this->now();
            message.driver[i].error_code = driver_four[i]->get_error_code();
            message.driver[i].temperature_c = driver_four[i]->get_temperature();
            message.driver[i].bus_voltage_v = driver_four[i]->get_voltage();
            message.driver[i].current_a = driver_four[i]->get_current();
            message.driver[i].power_w = driver_four[i]->get_power();
            message.driver[i].speed_rpm = driver_four[i]->get_speed();
            message.driver[i].position_rad = driver_four[i]->get_position();
        }
        message.header.stamp = this->now();
        publisher_->publish(message);
    }
    void pid_callback(const basic_sprayer_interfaces::msg::Pid::SharedPtr msg)
    {
        float p = msg->p;
        float i = msg->i;
        RCLCPP_INFO(this->get_logger(), "p=%f,i=%f", p, i);
        driver_four[0]->set_pi(p, i);
        driver_four[1]->set_pi(p, i);
        driver_four[2]->set_pi(p, i);
        driver_four[3]->set_pi(p, i);
    }
    void timer_callback()
    {
        // printf("INFO:1\n");
        /*500ms 进入一次*/
        if (comm_cnt == 0)
        {
            // printf("INFO:2\n");
            float speed[4] = {0};
            int8_t start = 0; //停止
            /*准备返回的指令*/
            auto message = basic_sprayer_interfaces::msg::FourDrivers();
            driver_four[0]->set_start(start);
            driver_four[0]->set_goal_speed(speed[0]);
            driver_four[1]->set_start(start);
            driver_four[1]->set_goal_speed(speed[1]);
            driver_four[2]->set_start(start);
            driver_four[2]->set_goal_speed(speed[2]);
            /*唤醒所有的线程*/
            pthread_mutex_lock(&mutex);
            pthread_cond_broadcast(&cond_send);
            pthread_mutex_unlock(&mutex);
            /* 需要读第四个驱动器 */
            if (driver_four[3]->read_write_start_stop(start, speed[3]) == false)
            {
                RCLCPP_INFO(this->get_logger(), "motor%d read_write_start_stop failed!", 3);
            }
            // printf("INFO:3\n");
            /* 等待所有线程结束 */
            sem_wait(&wait_end);
            // printf("INFO:5\n");
            sem_wait(&wait_end);
            sem_wait(&wait_end);
            // printf("INFO:4\n");
            for (uint8_t i = 0; i < NUM; i++)
            {
                // if (driver_four[i]->read_write_start_stop(start, speed[i]) == false)
                // {
                //     RCLCPP_INFO(this->get_logger(), "motor%d read_write_start_stop failed!", i);
                // }
                message.driver[i].header.stamp = this->now();
                message.driver[i].error_code = driver_four[i]->get_error_code();
                message.driver[i].temperature_c = driver_four[i]->get_temperature();
                message.driver[i].bus_voltage_v = driver_four[i]->get_voltage();
                message.driver[i].current_a = driver_four[i]->get_current();
                message.driver[i].power_w = driver_four[i]->get_power();
                message.driver[i].speed_rpm = driver_four[i]->get_speed();
                message.driver[i].position_rad = driver_four[i]->get_position();
            }
            message.header.stamp = this->now();
            publisher_->publish(message);
        }
        comm_cnt = 0;
    }
};

/**
* 运行ros2  100Hz  接收数据 读取数据  100Hz 如果达不到 再想办法
* 先要测试一下和四个驱动器通信的最高频率
* @author 何思伟
* @param 
*
* @note 
*
* @return 
*/
int main(int argc, char *argv[])
{
    /*打开第一个串口*/
    sem_init(&wait_end, 0, 0);
    /*创建 ros */
    rclcpp::init(argc, argv);
    auto pub_node = std::make_shared<driverNode>();
    rclcpp::spin(pub_node);
    /*创建线程*/
    rclcpp::shutdown();
}
